/*********************************************************************************

    @file      Engine_OPS.c                                                              
    @date      December, 2011
    @brief     Open5xxxECU - This file contains functions to initialize AND operate the eTPU
    @note      www.Open5xxxECU.org
    @version   3.4 
    @copyright MIT License

*/
/*=============================================================================*/
/* REV      AUTHOR         DATE          DESCRIPTION OF                        */
/* ---      -----------    ----------    ---------------------                 */
/* 3.4      M.Eberhardt    1/Jan/12      Added Used Config stuff               */
/* 3.3      J. Zeeff       29/Dec/11     Various cleanup                       */
/* 3.2      J. Zeeff       29/Oct/11     Add servo function                    */
/* 3.0      P. Schlein     22/Sep/11     Add Update_eTPU function              */
/* 2.1      P. Schlein     20/Aug/11     Remove SPARK from Compilation         */
/* 2.0      P. Schlein     20/June/11    Add 6 Channels of FUEL and SPARK      */
/* 1.0      P. Schlein     8/June/10     Initial ver.-eTPU Struct, init_eTPU   */
/*******************************************************************************/

/*  General                                                                    */
#include "config.h"
#include "system.h"

// 5602 has no eTPU
#ifndef NO_ETPU

#include "etpu_util.h"
#include "variables.h"
#include "main.h"

/*  eTPU Function Set                                                          */
#include "etpu_set.h"

/*  eTPU APIs                                                                  */
#include "etpu_toothgen.h"
#include "etpu_app_eng_pos.h"
#include "etpu_fuel.h"
#include "etpu_spark.h"
#include "etpu_pwm.h"
#include "Functions.h"
#define EXTERN
#include "eTPU_OPS.h"

#ifdef MPC5634
#include "mpc563m_vars.h"
#endif

#ifdef MPC5554
#include "mpc5554_vars.h"
#endif

/*******************************************************************************/
/* Global Variables                                                            */
static int32_t error_code;

/* First Test value of tcr1, engine=4,125,000; Prescaler=32                    */
#define etpu_tcr1_freq (CPU_CLOCK / 32)/2
uint32_t etpu_a_tcr1_freq = etpu_tcr1_freq;    // used by FreeScale code
uint32_t etpu_b_tcr1_freq = etpu_tcr1_freq;


/******************************************************************************************/
/*                                                                                        */
/* etpu_config structure filled according to the user settings                            */
/*                                                                                        */
/******************************************************************************************/

/*This structure follows the definition in eTPU_util.h that sets up the series-mcr, misc, */
/*ecr_a, tbcr_a, stacr_a, ecr_b, tbcr_b and stcr_b and defines the variables used below.  */
/*See also, AN2864, "General C Functions for eTPU"                                        */

struct etpu_config_t my_etpu_config = {
			          	/*eTPU MCR Register*/
  FS_ETPU_MISC_DISABLE,
                   /*MISC from compiler link file*/ 
  FS_ETPU_MISC,


			/*Configure eTPU Engine A ECR-see RM, pg18-23, 707*/
  FS_ETPU_FILTER_CLOCK_DIV8 + FS_ETPU_CHAN_FILTER_3SAMPLE + FS_ETPU_ENTRY_TABLE, 
  		/*Configure eTPU Engine A Timebases-see RM, 4.3.1, pg 4-9, 49-each Engine*/
  FS_ETPU_TCR2CTL_RISE +            /*TCR2CTL-set for RISE transition on TCRCLK           */
  (0<<27) +                         /*TCRCLK Filter /2 and 2 sample, table 4-10           */
  FS_ETPU_ANGLE_MODE_ENABLE+        /*Angle Mode enable = 0x02000000                      */
  (0<<16) +                         /*TCR2 prescaler of 1 (0+1)                           */
  FS_ETPU_TCR1CTL_DIV2 +            /*TCR1 PRESCALER OF 32 (31+1) APPLIED TO SYSCLK/2     */
  31,                               /*using the digital 31 is OK here; maybe unnecessary  */


                    /*Configure eTPU Engine A STAC CR*/
  FS_ETPU_TCR2_STAC_ENABLE +        /* TCR2 Resource Enable                               */
  FS_ETPU_TCR2_STAC_SERVER,         /*Engine A TCR2 Server                                */
 
 
 				    /*Configure eTPU Engine B ECR*/
  FS_ETPU_FILTER_CLOCK_DIV4 + FS_ETPU_CHAN_FILTER_3SAMPLE + FS_ETPU_ENTRY_TABLE, 
  	      /*Configure eTPU Engine B timebases-see RM, 4.3.1, pg 4-9, 49-each Engine*/
  (14 << 28) +                      /*TCR2 change as STAC client                          */ 
  FS_ETPU_TCR1CTL_DIV2 +            /*TCR1 PRESCALER OF 32 (31+1) APPLIED TO SYSCLK/2*/
  31,
                   /*Configure eTPU Engine B STAC CR*/
  FS_ETPU_TCR2_STAC_ENABLE +        /* TCR2 Resource Enable                               */
  FS_ETPU_TCR2_STAC_CLIENT +        /* Engine B TCR2 Client                               */
  2,                                /*Engine B listens for Engine A TCR2 on STAC buss     */
                                    /*this is SRV2--try to find details in documentation..*/
									
};

/******************************************************************************************/

/*********************************************************************************

    @brief   Set up eTPU(s)
    @note    This routine is NOT speed critical

*************************************************************************************/

int32_t init_eTPU()
{
    uint32_t i;
    uint24_t Cyl_Angle_eTPU[12];
    uint24_t Inj_End_Angle_eTPU;        //
    uint24_t Drop_Dead_Angle_eTPU;
    uint24_t Recalc_Angle_eTPU;
//ToDo - add cam window stuff to tuner variables
    uint24_t Cam_Window_Open = 360;
    uint24_t Cam_Window_Width;
    uint24_t Gen_Tooth_open;
    uint24_t Gen_Tooth_close;

    // Load firmware into eTPU
    fs_etpu_init(my_etpu_config,
                 (uint32_t *) etpu_code, sizeof(etpu_code),
                 (uint32_t *) etpu_globals, sizeof(etpu_globals));

    // Engine simulator for testing
    /* Initialize TOOTHGEN function (See AN2864,pg12)      */
#define GEN_RPM 100
#ifdef GEN_RPM
    //calculate a cam signal that works with the crank funtion setup
    Gen_Tooth_open = ((Cam_Window_Open+36000)/(36000/(N_Teeth + Missing_Teeth))+1) %(2*(N_Teeth + Missing_Teeth));
    Gen_Tooth_close = (Gen_Tooth_open+1) %(2*(N_Teeth + Missing_Teeth));
    Cam_Window_Width = 3*36000/(N_Teeth + Missing_Teeth);//make cam window 3 teeth wide to be certain gen signal is seen
    error_code =
        fs_etpu_toothgen_init(TOOTHGEN_PIN1, TOOTHGEN_PIN2,
                              FS_ETPU_PRIORITY_MIDDLE, N_Teeth, Missing_Teeth,
                              0x7fffff, 60, GEN_RPM, etpu_a_tcr1_freq, 30 , 32 );
    if (error_code != 0) {
        system_error(13379, __FILE__, __LINE__, "");
    }
#endif

    if (N_Cyl > 12)
        return -1;              // check for flash misconfig

#if 1

// Links cause a stall to notify some other channels and turn them off - 4 packed into a 32 bit word
#define Link1  0x01021313   // Cam must be linked then unused dummy - repeated as filler 
#define Link2  (Fuel_Channels[0] << 12) |  (Fuel_Channels[1] << 8) | (Fuel_Channels[2] << 4) | (Fuel_Channels[3] << 0) 
#define Link3  (Fuel_Channels[4] << 12) |  (Fuel_Channels[5] << 8) | (Fuel_Channels[6] << 4) | (Fuel_Channels[7] << 0) 
#define Link4  (Fuel_Channels[8] << 12) |  (Fuel_Channels[9] << 8) | (Fuel_Channels[10] << 4) | (Fuel_Channels[11] << 0)

    /*  Initialization of eTPU channel settings                                    */
    /* eTPU API Function Init: 'Engine Position (CAM and CRANK channels)'         */
    /* AN3769, pg16-18                                              */
    /* Note: crank on pin 0, cam on pin 1   */
    /*                                                                             */
    error_code = fs_etpu_app_eng_pos_init(1,    /*CAM in engine: A; channel: 1 */
                                          FS_ETPU_CAM_PRIORITY_MIDDLE,  /* cam_priority: Middle */
                                          0,    /* cam_edge_polarity: falling edge */
                                          53000,    /* cam_angle_window_start: cam_window_open*100   */
                                          3000, /* cam_angle_window_width: cam_window_width*100   */
                                          0,    /* CRANK in engine: A; channel: 0 */
                                          FS_ETPU_CRANK_PRIORITY_MIDDLE,        /* crank_priority: Middle */
                                          0,    /* crank_edge_polarity: falling edge */
                                          N_Teeth,      /* crank_number_of_physical_teeth: crank_number_of_physical_teeth */
                                          Missing_Teeth,        /* crank_number_of_missing_teeth: crank_number_of_missing_teeth */
                                          10,    /* crank_blank_tooth_count: crank_blank_tooth_count */
                                          60,   /* crank_tcr2_ticks_per_tooth: crank_tcr2_ticks_per_tooth */
                                          0x199999,     /* crank_windowing_ratio_normal: crank_windowing_ratio_normal */
                                          0x199999,     /* crank_windowing_ratio_after_gap: crank_windowing_ratio_after_gap */
                                          0x199999,     /* crank_windowing_ratio_across_gap: crank_windowing_ratio_across_gap */
                                          0x299999,     /* crank_windowing_ratio_timeout: crank_windowing_ratio_timeout */
                                          0x9fffff,     /* crank_gap_ratio: crank_gap_ratio */
                                          10,   /* crank_blank_time_ms: crank_blank_time_ms */
                                          40000,        /* crank_first_tooth_timeout_us: crank_first_tooth_timeout_us */
                                          Link1,        // a stall will notify these other channels
                                          Link2,
                                          Link3,      
                                          Link4,
                                          etpu_a_tcr1_freq);    /* tcr1_timebase_freq: etpu_a_tcr1_freq */

    if (error_code != 0) {
        system_error(13379, __FILE__, __LINE__, "");
    }

    // might be using staged injection
//removed Staged_Inj for testing
    N_Injectors = (N_Cyl + (N_Cyl * 0));// Staged_Inj)); removed for testing
    // might have waste spark
    N_Coils = (N_Cyl - ((N_Cyl * Ignition_Type) / 2));

    // Calculate the cylinder angles from user inputs
    for (i = 0; i < N_Cyl; ++i) {
        Cyl_Angle_eTPU[i] = ((Cyl_Offset_Array[i] + Engine_Position)<<2) % 72000;
    }
    Drop_Dead_Angle_eTPU = ((Drop_Dead_Angle + Engine_Position)<<2) % 72000;//

    // eTPU API Function initialization: 'fuel'-see AN3770, pg7-9
    error_code = fs_etpu_fuel_init_6cylinders(FUEL_CHANNELS_1_6,       //
                                              1,        // CAM in engine: A; channel: 1 
                                              FS_ETPU_PRIORITY_LOW, FS_ETPU_FUEL_FM0_ACTIVE_LOW,       //
                                              Cyl_Angle_eTPU[0],        //
                                              Cyl_Angle_eTPU[1],        //
                                              Cyl_Angle_eTPU[2],        // 
                                              Cyl_Angle_eTPU[3],        // 
                                              Cyl_Angle_eTPU[4],        //
                                              Cyl_Angle_eTPU[5],        // offset angles * 100
                                              21000,    // drop dead angle*100 
                                              0,    // normal end angle*100
                                              1500,     // recalc offset ANGLE*100
                                              0, 0, 0, 0, 0, 0,       //injection time, usec
                                              0,     //comp time, usec
                                              100,      //min inject time, usec
                                              100       //min injector off time, usec
        );
    if (error_code != 0) {
        system_error(12379, __FILE__, __LINE__, "");
    }

    if (N_Injectors > 6) {
        error_code =
            fs_etpu_fuel_init_6cylinders(FUEL_CHANNELS_7_12,
                                         // channels  
                                         1,     //CAM in engine: A; channel: 1 
                                         FS_ETPU_PRIORITY_LOW, FS_ETPU_FUEL_FM0_ACTIVE_HIGH,    //
                                         Cyl_Angle_eTPU[6], Cyl_Angle_eTPU[7], Cyl_Angle_eTPU[8], Cyl_Angle_eTPU[9], Cyl_Angle_eTPU[10], Cyl_Angle_eTPU[11],    // offset angles * 100
                                         21000, // drop dead angle*100
                                         20000, // normal end angle*100
                                         1500,  // recalc offset ANGLE*100
                                         1000, 1000, 1000, 1000, 1000, 1000,    //injection time, usec
                                         1000,  //comp time, usec
                                         100,   //min inject time, usec
                                         100    //min injector off time, usec
            );
        if (error_code != 0) {
            system_error(12379, __FILE__, __LINE__, "");
        }
    }
// MPC5554 can have more than 12 fuel
#ifdef MPC5554
    if (N_Injectors > 12) {
        error_code =
            fs_etpu_fuel_init_6cylinders(FUEL_CHANNELS_13_18,
                                         // channels  
                                         1,     //CAM in engine: A; channel: 1 
                                         FS_ETPU_PRIORITY_LOW, FS_ETPU_FUEL_FM0_ACTIVE_HIGH, Cyl_Angle_eTPU[12], Cyl_Angle_eTPU[13], Cyl_Angle_eTPU[14], Cyl_Angle_eTPU[15], Cyl_Angle_eTPU[16], Cyl_Angle_eTPU[17],    // offset angles * 100
                                         21000, // drop dead angle*100
                                         20000, // normal end angle*100
                                         1500,  // recalc offset ANGLE*100
                                         1000, 1000, 1000, 1000, 1000, 1000,    //injection time, usec
                                         1000,  //comp time, usec
                                         100,   //min inject time, usec
                                         100    //min injector off time, usec
            );
        if (error_code != 0) {
            system_error(12379, __FILE__, __LINE__, "");
        }
    }

    if (N_Injectors > 18) {
        error_code = fs_etpu_fuel_init_6cylinders(FUEL_CHANNELS_19_24, 1,      //CAM in engine: A; channel: 1 
                                                  FS_ETPU_PRIORITY_LOW, FS_ETPU_FUEL_FM0_ACTIVE_HIGH,   // offset angles*100
                                                  Cyl_Angle_eTPU[18], Cyl_Angle_eTPU[19], Cyl_Angle_eTPU[20], Cyl_Angle_eTPU[21], Cyl_Angle_eTPU[22], Cyl_Angle_eTPU[23],       // offset angles * 100
                                                  21000,        // drop dead angle*100
                                                  20000,        // normal end angle*100
                                                  1500, // recalc offset ANGLE*100
                                                  1000, 1000, 1000, 1000, 1000, 1000,   //injection time, usec
                                                  1000, //comp time, usec
                                                  100,  //min inject time, usec
                                                  100   //min injector off time, usec
            );
        if (error_code != 0) {
            system_error(12379, __FILE__, __LINE__, "");
        }
    }
#endif
//turn off injectors until called
               // for (i = 0; i < N_Injectors; ++i) {
               //    error_code = 1; 
               //    while (error_code != 0)// This tries until the channel actual updated
               //    {
               //    error_code = fs_etpu_fuel_switch_off(Fuel_Channels[i]);     // Turn on fule channels
               //    }
               // }

    // -----------------------------------------------------------------
    // eTPU API Function initialization: 'spark'
    // in cylinder order not firing order

    error_code = fs_etpu_spark_init_6cylinders(SPARK_CHANNELS_1_6,     //
                                               1,       //CAM in engine: A; channel: 1
                                               // offset angles*100
                                               Cyl_Angle_eTPU[0],       //
                                               Cyl_Angle_eTPU[1],       //
                                               Cyl_Angle_eTPU[2],       //
                                               Cyl_Angle_eTPU[3],       // 
                                               Cyl_Angle_eTPU[4],       //
                                               Cyl_Angle_eTPU[5],       // offset angles * 100
                                               FS_ETPU_PRIORITY_MIDDLE, FS_ETPU_SPARK_FM0_ACTIVE_LOW, 
                                               0,       //min_coil_dur, usec 
                                               300000,  //max_coil_dur, usec 
                                               0,       //multi spark on time 
                                               0,       //multi spark on time
                                               0,       //multi nmbr pulses
                                               1500,    // recalc offset angles*100
                                               0,  //init dwell time 1, usec 
                                               (1* Ignition_Type), //init dwell time 2, usec, 0 if direct fire
                                               0,       //init end angle 1*100
                                               36000    //init end angle 2*100                                           
        );

    if (error_code != 0) {
        system_error(12389, __FILE__, __LINE__, "");
    }

// 5554 can have more
#ifdef MPC5554
    if ((N_Coils > 6) && (N_Injectors < 19)) {

        error_code = fs_etpu_spark_init_6cylinders(SPARK_CHANNELS_7_12, 1,   //CAM in engine: A; channel: 1
                                                   Cyl_Angle_eTPU[6], Cyl_Angle_eTPU[7], Cyl_Angle_eTPU[8], Cyl_Angle_eTPU[9], Cyl_Angle_eTPU[10], Cyl_Angle_eTPU[11],  // offset angles * 100
                                                   FS_ETPU_PRIORITY_MIDDLE, FS_ETPU_SPARK_FM0_ACTIVE_HIGH, 
                                                   0,      //min_coil_dur, usec 
                                                   4000,   //max_coil_dur, usec
                                                   500,    //multi spark on time 
                                                   0,      //multi spark on time
                                                   0,      //multi nmbr pulses
                                                   1500,   // recalc offset angles*100
                                                   3000,   //init dwell time 1, usec 
                                                   (3000 * Ignition_Type),      //init dwell time 2, usec, 0 if direct fire
                                                   5000,    //init end angle 1*100
                                                   41000    //init end angle 2*100                                           
            );

        if (error_code != 0) {
            system_error(12389, __FILE__, __LINE__, "");
        }

    }
#endif

#if 1
// Have the eTPU generate trigger pulse for reading MAP values.  
// Normally this feature is used for knock windows, 
// but we use it for triggering a MAP A/D sampling

#define WINDOW0_CHANNEL   26  // eTPU channel to output windows on (and trigger A/D)

N_MAP_windows = N_Cyl;
if (N_MAP_windows > 8)
  N_MAP_windows /= 2;     // limit is 8 so space them evenly (divide by 2)

// define initial windows in degrees x 100 (will be adjusted later)
for (i = 0; i < N_MAP_windows; ++i) {
    MAP_Angles[i*2] = Cyl_Angle_eTPU[i];  /* Window (open)  */
    MAP_Angles[i*2] = i * 3000;  /* TODO testing only  */
    MAP_Angles[i*2+1] = 10;    			/* Window (width) */
}  // assume unused ones don't matter

#include "etpu_knock_window.h"
#include "etpu_knock_window_auto.h"

fs_etpu_knock_window_init (
        WINDOW0_CHANNEL, 			/*  */
        FS_ETPU_PRIORITY_LOW, 		/* priority: */   
        N_MAP_windows, 				/* number_of_windows: */
        FS_ETPU_KNOCK_FM0_RISING_EDGE, 	/* edge_polarity: */ 
        FS_ETPU_KNOCK_FM1_INT_OPEN,	/* edge_interrupt: */
        1, 							/* cam_chan: 1 */
        MAP_Angles); 				/* window_angle_table: */
#endif

#endif

    return 0;

}                               // init_eTPU()

/****************************************************************************

   @note Use the eTPU to maintain a single channel servo or PWM valve position.

   @note Standard servo motors use 50Hz and duty cycle can range from 5% to 10% (500-1000 or 1-2 msec)
   @note Ford PWM idle valves run best at a frequency of around 300-320 Hz with 0-100% duty cycle
   @note Bosch 3 wire PWM idle valves run best at a frequency of 200 Hz with 10-80% duty cycle
   @param Supply duty cycle % x 100

   @ note - it would probably be better to use the eMIOS for this

*****************************************************************************/

static uint32_t PWM1_frequency;

void init_PWM1(uint32_t frequency)
{
    // start with ~0% DC
    error_code =
        fs_etpu_pwm_init(PWM1_CHANNEL, FS_ETPU_PRIORITY_LOW, frequency,
                         1, FS_ETPU_PWM_ACTIVEHIGH,
                         FS_ETPU_TCR1, etpu_a_tcr1_freq);
    if (error_code != 0)
        system_error(24579, __FILE__, __LINE__, "");

    PWM1_frequency = frequency;
}

void update_PWM1(uint32_t duty_cycle)
{

    if (duty_cycle > 10000)     // clip to 100% DC
        duty_cycle = 10000;

    // update to new duty cycle
    error_code =
        fs_etpu_pwm_update(PWM1_CHANNEL, PWM1_frequency, duty_cycle,
                           etpu_a_tcr1_freq);

    if (error_code != 0)
        system_error(34379, __FILE__, __LINE__, "");

}                               // update_PWM1()

/****************************************************************************

   @note Use the eTPU to Output an RPM signal.

*****************************************************************************/

void Init_Tach(void)
{
    error_code =
        fs_etpu_pwm_init(TACH_CHANNEL, FS_ETPU_PRIORITY_LOW,1,
                         1000, FS_ETPU_PWM_ACTIVEHIGH,
                         FS_ETPU_TCR1, etpu_a_tcr1_freq);
    if (error_code != 0)
        system_error(24579, __FILE__, __LINE__, "");

}

void Update_Tach(uint32_t frequency)
{
    if (frequency < 1) frequency = 1;  // avoid illegal value

    // update to new freq
    error_code =
        fs_etpu_pwm_update(TACH_CHANNEL, frequency, 1000,etpu_a_tcr1_freq);

    if (error_code != 0)
        system_error(34379, __FILE__, __LINE__, "");

}           

#endif
